// ----------------------------------------
// Filename: Scheduler.c
// Description: The code that implements the kernel's Scheduler
// Author: Jon Maloney
// Date: 9-May-2012
//

//************************************************************
//                    Include .h files
//************************************************************
#include "scheduler.h"
#include "Timer.h"
#include <stdlib.h>
//************************************************************
//                    Include external references
//************************************************************

//************************************************************
//                    Define Statements
//************************************************************

//************************************************************
//                    Struct, Union, enum Statements
//************************************************************

//************************************************************
//                    Global variables and constants
//************************************************************
static T_READY_QUEUE readyQueue;
static linkedList_Handle waitingQueue;
PRIVILEGED_DATA TCB * volatile currentRunningTask;
static linkedList_Handle currentRunningTaskNode;
static kernelTickType kernelTickCount;

kernelBASE_TYPE uxCriticalNesting = 0xef;

//************************************************************
//                    Prototype Private Functions
//************************************************************

void scheduler_TickIncrement(void);

//************************************************************
//                    Implementation
//************************************************************

// ----------------------------------------
// TIMER 1 ISR
// 
// Interrupts at the frequency of the tick
//	count. Is responsible for ensuring that the
//	scheduler_TickIncrement funciton is run
//	at the correct frequency.
//
// Input:
//	none
// Output:
//  none
// Conditions:
// 	none
void __attribute__((__interrupt__, auto_psv)) _T1Interrupt( void )
{
	/* Clear the timer interrupt. */
	IFS0bits.T1IF = 0;
	ENABLE_INTERRUPTS();
	
	scheduler_TickIncrement();

	//A context switch needs to be performed
	Scheduler_PreEmptiveContextSwitch();
}

// ----------------------------------------
// scheduler_GetTickCount
// 
// Returns the current system tick Count
//
// Input:
//	none
// Output:
//  The current tick value
// Conditions:
// 	none
kernelTickType Scheduler_GetTickCount()
{
	return kernelTickCount;
}

// ----------------------------------------
// Scheduler_DeleteTask
// 
// Deletes a task from the system
//
// Input:
//	none
// Output:
//  The current tick value
// Conditions:
// 	none
void Scheduler_DeleteTask(linkedList_Handle taskNode)
{
	LinkedList_DeleteNode(taskNode);
}

// ----------------------------------------
// scheduler_TickIncrement
// 
// Increments the tick count and checks
//	to see if there are any items in the waiting
//	queue that need to be moved to the ready queue
//
// Input:
//	none
// Output:
//  none
// Conditions:
// 	none
void scheduler_TickIncrement(void)
{
	TCB * waitingQueueTask;
	linkedList_Handle nodeToAddToWaitingQueue;
	kernelTickCount++;	//Increment the kernels tick count
	BOOL contextSwitchNeeded = pdFALSE;
	//If the waiting queue is empty
	if(waitingQueue == NULL)
		return;

	ENTER_CRITICAL();

	//Get the first task from the waiting queue.
	waitingQueueTask = (TCB *)waitingQueue->Item;

	//Check to see if any tasks in the waiting queue need to be placed in the ready queue		
	while(waitingQueueTask->WakeOnTickValue == kernelTickCount)
	{//The task is ready to be woken
		
		//Remove the task node from the waiting queue
		if(LinkedList_Remove(waitingQueue) == pdFALSE)
		{// If there are no items left in the waiting queue
			
			//Place the task in the ready queue to wake the task
			ReadyQueue_AddNode( &readyQueue, waitingQueue, waitingQueueTask->Priority );
			
			waitingQueue = NULL;

			break;		
		}
		else //Items remain the the waiting queue
		{
			//Find the next node in the waiting queue
			nodeToAddToWaitingQueue = (linkedList_Handle)waitingQueue->NextNode;

			//Place the task in the ready queue to wake the task
			if(ReadyQueue_AddNode( &readyQueue, waitingQueue, waitingQueueTask->Priority ) == pdTRUE)
				contextSwitchNeeded = pdTRUE;
			
			//Update the waiting queue
			waitingQueue = nodeToAddToWaitingQueue;

			//Get a reference to the next thread in the waiting queue
			waitingQueueTask = (TCB *)waitingQueue->Item;
		}
	}
	
	EXIT_CRITICAL();

	if(contextSwitchNeeded == pdTRUE)
		Scheduler_PreEmptiveContextSwitch();

}

// ----------------------------------------
// Scheduler_FindPriorityTask
// 
// Finds the highest priority thead in the queue. 
// Input:
//  taskList: A pointer to the list of tasks from which
//				the highest priority task will be found
// Output:
//   A handle to the TCB of the highest priority task found in the list.
// Conditions:
// 	none
inline TCB * Scheduler_FindPriorityTask( T_READY_QUEUE * rQueue )
{	
	//Get the highest priority task from the ready queue		
	return (TCB *)ReadyQueue_GetHighestPriorityItem(rQueue);
}

// ----------------------------------------
// Scheduler_AddTaskToReadyQueue
// 
// Adds a task to the ready Queue
// Input:
//  taskList: A pointer to the task to be added
//				to the ready queue
// Output:
//   A handle to the task node used by the scheduler 
//		to track the task.
// Conditions:
// 	none
linkedList_Handle Scheduler_AddTaskToReadyQueue(TCB * task)
{
	linkedList_Handle node;
	
	//Insert the task node into the ready queue
	if(ReadyQueue_Add( &readyQueue, node, (void *)task, task->Priority ) == eQueueErrorAddFailure)
		return NULL;
	
	//Change the state of the task
	task->currentState = eReady;

	return node;
}

// ----------------------------------------
// Scheduler_AddTaskNodeToReadyQueue
// 
// Adds a task to the ready Queue
// Input:
//  taskList: A pointer to the list node of tasks from which
//				which contains the task to be moved.
// Output:
//   none
// Conditions:
// 	none
BOOL Scheduler_AddTaskNodeToReadyQueue(linkedList_Handle taskToAddNode)
{
	TCB * task = (TCB *)taskToAddNode->Item;
	
	//Insert the task node into the ready queue
	ReadyQueue_AddNode( &readyQueue, taskToAddNode, task->Priority );

	//Change the state of the task
	task->currentState = eReady;

	return pdTRUE;
}

// ----------------------------------------
// Scheduler_MoveTaskNodeToReadyQueue
// 
// Moves a task node from the queue it is contained in to the ready Queue
// Input:
//  taskList: A pointer to the list node where the task is located.
// Output:
//   none
// Conditions:
// 	none
BOOL Scheduler_MoveTaskNodeToReadyQueue(linkedList_Handle taskToAddNode)
{
	TCB * task = (TCB *)taskToAddNode->Item;
	
	ENTER_CRITICAL();
	//If the task is in the waiting queue
	if(task->currentState == eWaiting)
	{
		//Remove the task from the waiting queue.
		(void)LinkedList_Remove(taskToAddNode);	
	}
	
	//If the task is in the waiting queue or the running state
	if (task->currentState != eReady)
	{
		//Insert the task node into the ready queue
		ReadyQueue_AddNode( &readyQueue, taskToAddNode, task->Priority );

		//Change the state of the task
		task->currentState = eReady;
	}
	
	EXIT_CRITICAL();
	return pdTRUE;
}

// ----------------------------------------
// Scheduler_DemoteTask
// 
// To be used in conjunciton with TemporarilyPromoteThread
//	this funciton will restore the task back to its original priority
//	
// Input:
//  taskList: A pointer to the list node where the task is located.
// Output:
//   none
// Conditions:
// 	The task must be on the ready queue before this function is called.
//	This is different from ready queue move because ready queue move 
//	protects the ready queue by preventing tasks not on the ready queue
//	from being removed. This fucntion has no such protection.
void Scheduler_DemoteTask(linkedList_Handle taskToAddNode)
{
	TCB * task = (TCB *)taskToAddNode->Item;
	
	ENTER_CRITICAL();
	
	//Remove the task from its promoted position in the ready queue
	ReadyQueue_Remove(&readyQueue, taskToAddNode, task->Priority);

	//Insert the task node into the ready queue at its priority position
	ReadyQueue_AddNode( &readyQueue, taskToAddNode, task->Priority );
	
	EXIT_CRITICAL();
}

// ----------------------------------------
// Scheduler_GetNextTaskFromReadyQueue
// 
// Gets the next highest priority 
//	task from the ready queue and 
//	returns a handle to this task.
// Input:
// 	none
// Output:
//   none
// Conditions:
// 	none
inline TCB *Scheduler_GetNextTaskFromReadyQueue(void)
{
	return (TCB *)ReadyQueue_GetAndRemoveHighestPriorityItem(&readyQueue);
}

// ----------------------------------------
// Scheduler_GetNextTaskNodeFromReadyQueue
// 
// Gets the next highest priority 
//	task node from the ready queue and 
//	returns a handle to this task node.
// Input:
// 	none
// Output:
//   none
// Conditions:
// 	none
inline linkedList_Handle Scheduler_GetNextTaskNodeFromReadyQueue(void)
{
	return ReadyQueue_GetAndRemoveHighestPriorityNode(&readyQueue);
}

// ----------------------------------------
// Scheduler_PreEmptiveContextSwitch
// 
//  If there is a higher priority task in the 
//	waiting queue a context switch will be performed.
//	
//	When performing a context switch the the currently running
//	task is placed on the ready queue and the next highest priority
//	task is switched in.
//
//	If there is a task on the ready queue of equal priority to the task
//	running a context switch will be performed.
//
// Input:
// 	none
// Output:
//   none
// Conditions:
// 	none
void Scheduler_PreEmptiveContextSwitch( void )
{
	ENTER_CRITICAL();
	
	//If the currently running task has a lower or equivilant priority to the
	//	highest priority task in the ready queue we need to perform a context switch.
	if(currentRunningTask->Priority < readyQueue.higestPriorityElementInQueue)
	{	
		//Move the currently running task to the ready queue
		//ReadyQueue_AddNode( &readyQueue, currentRunningTaskNode, currentRunningTask->Priority );
		
		//Update the state of the task
		currentRunningTask->currentState = eReady;

		//Save the context of the currently running task
		//	Switch in the next highest priority task from the ready queue
		//	Load the context of the next highest priority task
		ContextSwitch();
	}

	EXIT_CRITICAL();
}

// ----------------------------------------
// Scheduler_PreEmptiveContextSwitch
// 
//  If the currently running task is in the running state
//	a pre-emptive context switch will be performed. See
//	(Scheduler_PreEmptiveContextSwitch)
//
//	If the currently running task is no longer running
//	the next highest priority task will be switched in.
//
// Input:
// 	none
// Output:
//   none
// Conditions:
// 	none
void Scheduler_TaskYeild( void )
{
	ENTER_CRITICAL();
	
	//Make sure that the currently running task is no longer running
	//	before performing a context switch. If it is still running
	//	we need to move it to the ready queue.
	//
	//If the task was supposed to be in the waiting state at this point 
	//	it should have already been placed in the waiting queue.
	if(currentRunningTask->currentState == eRunning)
	{
		EXIT_CRITICAL();
		void Scheduler_PreEmptiveContextSwitch( void );
	}
	else
	{
		EXIT_CRITICAL();
		//Save the context of the currently running task
		//	Switch in the next highest priority task
		//	Load the context of the next highest priority task
		ContextSwitch();
	}
}

// ----------------------------------------
// Scheduler_SwitchInNextPriorityTask
// 
//  This function is only to be used by the contextSwitch function.
//	
//	The function gets the next highest priority task from the ready queue
//	and makes this task the currently running task.
//
// Input:
// 	none
// Output:
//   none
// Conditions:
// 	none
void Scheduler_SwitchInNextPriorityTask( void )
{
	//Get the next highet priority node from the ready queue
	currentRunningTaskNode = readyQueue.rQueues[readyQueue.higestPriorityElementInQueue];
	//***OLD CODE currentRunningTaskNode = ReadyQueue_GetAndRemoveHighestPriorityNode(&readyQueue);

	//Update the head of the ready queue
	readyQueue.rQueues[readyQueue.higestPriorityElementInQueue] = (linkedList_Handle)currentRunningTaskNode->NextNode;
	
	//Get the TCB from the node.
	currentRunningTask = (TCB *)currentRunningTaskNode->Item;

	//Set the state of the currently running task to running
	currentRunningTask->currentState = eRunning;	
}

// ----------------------------------------
// Scheduler_Delay
// 
//  Loads the currently running task into the waiting queue
//	for the delayTime.
//
// Input:
// 	delayTime: 	The time in ticks that the current task is to be
//				delayed for.
// Output:
//   none
// Conditions:
// 	none
void Scheduler_Delay( kernelTickType delayTime )
{
	TCB * queuedTask;
	UINT16 queuedTaskDelayTime;
	linkedList_Handle tempWaitingQueue;
	
	ENTER_CRITICAL();
	
	//Keep track of when the task will be woken
	currentRunningTask->WakeOnTickValue = delayTime + kernelTickCount;
	
	//Remove the node from the ready queue
	(void)ReadyQueue_Remove( &readyQueue, currentRunningTaskNode, currentRunningTask->Priority );
	
	if(kernelTickCount == 190)
		asm volatile("NOP");

	//If the waiting queue is empty
	if( waitingQueue == NULL )
	{
		//Place the currently running task into the waiting queue at the first position
		waitingQueue = LinkedList_Insert(waitingQueue, currentRunningTaskNode);
	}
	else
	{
		//We need to look into the waiting queue without 
		//	changing the pointer to the first node in the 
		//	waiting queue.
		tempWaitingQueue = waitingQueue;
	
		queuedTask = (TCB *)tempWaitingQueue->Item;
	
		queuedTaskDelayTime = queuedTask->WakeOnTickValue - kernelTickCount;

		//if the current running task needs to be at the front of the waiting queue
		//	Does the first thread in the queue have a larger delay than the task
		//	requesting to be delayed.
		if(queuedTaskDelayTime > delayTime)
		{
			//Place the currently running task into the start of the waiting queue
			tempWaitingQueue = LinkedList_Insert( waitingQueue, currentRunningTaskNode );
			//Update position the ready queue.
			waitingQueue = currentRunningTaskNode;

		}
		else
		{	

			//Find the point in the waiting queue to place the currently running task.
			// 	If all tasks in the queue have the same wake on tick time value and 
			//	the same priority as the currently running task the currently running 
			//	task will be placed at the back of the queue. This ensures when the 
			//	tasks need to be woken they are placed into the ready queue in the same 
			//	order that they were placed in the waiting queue. This means the task
			//	that has been in the waiting queue the longest will get access to the 
			//	CPU first.
			//
			do
			{
				//look at the next task in the waiting queue
				//	Look from the back of the queue to the front
				tempWaitingQueue = (linkedList_Handle)tempWaitingQueue->PreviousNode;
				queuedTask = (TCB *)tempWaitingQueue->Item;
				queuedTaskDelayTime = queuedTask->WakeOnTickValue - kernelTickCount;

				//If the task in the waiting queue that has a smaller wait on tick
				//	 value than our currently running task.
				if( delayTime > queuedTaskDelayTime )
					break;//We have found the position to put the current task
				
			}while(tempWaitingQueue != waitingQueue);	//Only loop through the queue once.
			
			//Place the currently running task into the waiting queue at the point found
			LinkedList_Insert((linkedList_Handle)tempWaitingQueue->NextNode, currentRunningTaskNode);
		}
	}
	
	//Update the state of the task it is now waiting.
	currentRunningTask->currentState = eWaiting;

	EXIT_CRITICAL();
	
	Scheduler_TaskYeild();	
}

// ----------------------------------------
// Scheduler_GetCurrentlyRunningTask
// 
//  Returns a pointer to the currently running task's TCB
//
// Input:
// 	None
// Output:
//   A pointer to the currently running task's TCB
// Conditions:
// 	none
TCB * Scheduler_GetCurrentlyRunningTask(void)
{
	return currentRunningTask;
}

// ----------------------------------------
// Scheduler_GetCurrentlyRunningTaskNode
// 
//  Returns a pointer to the currently running task's link list node
//
// Input:
// 	None
// Output:
//   A pointer to the currently running task's link list node
// Conditions:
// 	none
linkedList_Handle Scheduler_GetCurrentlyRunningTaskNode(void)
{
	return currentRunningTaskNode;
}

// ----------------------------------------
// Scheduler_TemporarilyPromoteThread
// 
//  Temporarily promotes a threads priority. Promotion is done
//	by placing it on the ready queue at a higher priority level.
//
//	Note: 	The funciton assumes that p is of higher priority than
//			the task priority. If p is a lower priorty than the task
//			priority then the task will actually be demoted.
//
//			The task will only be promoted for 1 time quantum
//			during this time it will get the opportunity to finish
//			what it has to do and releas its resource lock.
//
// Input:
// 	taskNode:	A pointer to the link list node that contains the 
//				task which is to be promoted.
//	p:			The priority that the task is to be promoted to.
// Output:
//   A pointer to the currently running task's link list node
// Conditions:
// 	none
BOOL Scheduler_TemporarilyPromoteThread( linkedList_Handle taskNode, PRIORITY p)
{
	TCB * task = (TCB *)taskNode->Item;
	/*Move the task that is to be promoted to the ready queue*/
	
	//If the task is in the waiting queue
	if(task->currentState == eWaiting)
	{
		//Remove the task from the waiting queue.
		(void)LinkedList_Remove(taskNode);	
	}

	//If the task was at the head of the waiting queue
	//	update the waiting queue.
	if(taskNode == waitingQueue)
	{
		waitingQueue = waitingQueue->NextNode;
	}
	
	//If the task is already in the ready queue.
	//	It will be in the ready queue at the wrong priority
	//	level.
	if (task->currentState == eReady)
	{
		ReadyQueue_Remove( &readyQueue, taskNode, task->Priority );
	}
	
	//Insert the task node into the ready queue at priority p (New priority)
	ReadyQueue_AddNode( &readyQueue, taskNode, p );

	//move the added node to the head of the queue
	readyQueue.rQueues[p] = taskNode;

	//Change the state of the task
	task->currentState = eReady;
	
	if(p >= readyQueue.higestPriorityElementInQueue)
	{
		//Update the state of the task
		currentRunningTask->currentState = eReady;

		//Save the context of the currently running task
		//	Switch in the next highest priority task from the ready queue
		//	Load the context of the next highest priority task
		ContextSwitch();
		return pdTRUE;
	}

	return pdTRUE;
}

// ----------------------------------------
// Scheduler_Start
// 
//  Starts the kernels scheduler
//	the function will never return from here.
//
// Input:
// 	none:
// Output:
//   none:
// Conditions:
// 	The threads the you want to run after this function is called
//	have to have been created before this function is called.
void Scheduler_Start( void )
{
	/* Setup a timer for the tick ISR. */
	Timer_SetupTickTimerInterrupt(); 
	
	//initialise the tick count.
	kernelTickCount = 0;

	//Initialise the waiting queue
	waitingQueue = NULL;
	
	//Get the highest priority task node from the ready queue
	currentRunningTaskNode = readyQueue.rQueues[readyQueue.higestPriorityElementInQueue];
	
	//Get the task from the task node
	currentRunningTask = (TCB *)currentRunningTaskNode->Item;
	
	//Set the task state to running.
	currentRunningTask->currentState = eRunning;

	/* Restore the context of the first task to run. */
	RESTORE_CONTEXT();
	
	/* Simulate the end of the yield function. */
	asm volatile ( "return" );
	
}
